#include <math.h>

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Int8.h>
#include <std_msgs/String.h>

#define STATUS_EXPLORING (0)
#define STATUS_CLOSE_TARGET (1)
#define STATUS_GO_HOME (2)

#define GET_TARGET_SIZE (50000)

ros::Publisher vel_pub;
ros::Publisher cmd_pub;
ros::Publisher voice_pub;
// 定义标志位，此标志位意味着只要识别到宝藏，则宝藏状态标志位不会改变
int find_once_flag = 0;
int status_flag = STATUS_EXPLORING;
// 接收到订阅的消息后，会进入消息回调函数
void poseCallback(const geometry_msgs::Pose::ConstPtr &msg)
{
    // 将接收到的消息打印出来
    ROS_INFO("Target pose: x:%0.6f, y:%0.6f, z:%0.6f", msg->position.x, msg->position.y, msg->position.z);

    /******************************请补充此处代码（开始）*******************************************/
    // 未发现宝藏的情况下，宝藏状态置0，并发布
    if (find_once_flag != 1)
    {
        std_msgs::Int8 expore_flag;
        expore_flag.data = 0;
        cmd_pub.publish(expore_flag);
    }
    // 已发现宝藏情况下，宝藏状态置1，并不会再度改变
    if (msg->position.z > GET_TARGET_SIZE || find_once_flag == 1)
    {
        std_msgs::String msg;
        msg.data = "拜拜";
        voice_pub.publish(msg);
        find_once_flag = 1;
        std_msgs::Int8 find_flag;
        find_flag.data = 1;
        cmd_pub.publish(find_flag);
    }
    /******************************请补充此处代码（结束）*******************************************/
}

int main(int argc, char **argv)
{
    // ROS节点初始化
    ros::init(argc, argv, "move_to_target");

    // 创建节点句柄
    ros::NodeHandle n;

    // 创建一个Subscriber，订阅名为/turtle1/pose的topic，注册回调函数poseCallback
    ros::Subscriber pose_sub = n.subscribe("/object_detect_pose", 10, poseCallback);
    // 创建一个Publisher，发布名为cmd_vel的topic，消息类型为std_msgs::Int8，队列长度10
    cmd_pub = n.advertise<std_msgs::Int8>("/exploring_cmd", 10);
    // 发布语音输出内容
    voice_pub = n.advertise<std_msgs::String>("/voiceWords", 1000);

    // 循环等待回调函数
    ros::spin();

    return 0;
}
